/*
 * fifo.c
 *
 *  Created on: 2021年7月25日
 *      Author: Administrator
 */

#include "fifo.h"

#define DEBUG_TAG "fifo,%d", __LINE__
#define DEBUG_LEVEL ERROR_OUTPUT
#include "debug.h"

/**
 * 获取FIFO中的数据长度
 *
 * @param fifo FIFO句柄
 *
 * @return 返回FIFO中的数据长度
 */
static size_t __fifo_lenght(fifo_t *fifo)
{
    if(fifo->r_index == fifo->w_index) {
        return fifo->is_empty ? 0 : fifo->size;
    }
    else if(fifo->w_index > fifo->r_index) {
        return fifo->w_index - fifo->r_index;
    }
    else {
        return fifo->size - fifo->r_index + fifo->w_index;
    }
}

/**
 * 更新FIFO的r_index
 *
 * @param fifo FIFO句柄
 * @param size 刚从FIFO中读取的数据长度
 *
 * @return 无
 */
static void __update_r_index(fifo_t *fifo, size_t size)
{
    if(size >= __fifo_lenght(fifo)) {
        fifo->is_empty = 1;
        fifo->r_index = fifo->w_index;
    }
    else {
        fifo->r_index += size;
        if(fifo->r_index >= fifo->size) {
            fifo->r_index %= fifo->size;
        }
    }
}

/**
 * 更新FIFO的w_index
 *
 * @param fifo FIFO句柄
 * @param size 刚写入FIFO的数据长度
 *
 * @return 无
 */
static void __update_w_index(fifo_t *fifo, size_t size)
{
    if((fifo->r_index == fifo->w_index) && (!fifo->is_empty)) {
        /* FIFO已是满的 */
        fifo->w_index += size;
        if(fifo->w_index >= fifo->size) {
            fifo->w_index %= fifo->size;
        }
        fifo->r_index = fifo->w_index;
    }
    else if(fifo->r_index <= fifo->w_index) {
        fifo->w_index += size;
        if(fifo->w_index >= fifo->size) {
            fifo->w_index %= fifo->size;
            if(fifo->w_index >= fifo->r_index) {
                fifo->r_index = fifo->w_index;
                fifo->is_empty = 0;
            }
        }
    }
    else {
        fifo->w_index += size;
        if(fifo->w_index >= fifo->r_index) {
            if(fifo->w_index >= fifo->size) {
                fifo->w_index %= fifo->size;
            }
            fifo->r_index = fifo->w_index;
            fifo->is_empty = 0;
        }
    }
}

/**
 * 将数据强制写入FIFO
 *
 * @param fifo FIFO对象
 * @param data 待写入的数据
 * @param size 数据长度
 *
 * @return 返回成功写入的字节数
 */
static ssize_t __fifo_in_force(fifo_t *fifo, const void *data, size_t size)
{
    size_t temp_bytes;
    const uint8_t *pdata = (const uint8_t*)data;
    uint8_t *ppool = (uint8_t*)fifo->pool;
    
    if(size > fifo->size) {
        size =  fifo->size;
    }
    
    temp_bytes = fifo->size - fifo->w_index;
    if(size > temp_bytes) {
        memcpy(&ppool[fifo->w_index], &pdata[0], temp_bytes);
        memcpy(&ppool[0], &pdata[temp_bytes], size-temp_bytes);
    }
    else {
        memcpy(&ppool[fifo->w_index], &pdata[0], size);
    }
    
    __update_w_index(fifo, size);
    return (ssize_t)size;
}

/**
 * 从FIFO读取数据
 *
 * @param fifo FIFO句柄
 * @param buf 存放数据的缓存
 * @param size 期望读取的字节数
 *
 * @return 返回成功读取的字节数
 */
static ssize_t __fifo_out(fifo_t *fifo, void *buffer, size_t size)
{
    size_t temp_bytes;
    uint8_t *pbuffer = (uint8_t*)buffer;
    uint8_t *ppool = (uint8_t*)fifo->pool;

    temp_bytes = __fifo_lenght(fifo);
    if(size > temp_bytes) {
        size = temp_bytes;
    }
    
    temp_bytes = fifo->size - fifo->r_index;
    if(size > temp_bytes) {
        memcpy(&pbuffer[0], &ppool[fifo->r_index], temp_bytes);
        memcpy(&pbuffer[temp_bytes], &ppool[0], size-temp_bytes);
    }
    else {
        memcpy(&pbuffer[0], &ppool[fifo->r_index], size);
    }
    
    __update_r_index(fifo, size);
    return (ssize_t)size;
}

/**
 * 创建FIFO
 *
 * @param size FIFO大小
 *
 * @return 运行成功返回FIFO，失败返回NULL
 */
fifo_t *fifo_create(size_t size)
{
    fifo_t *fifo;
    void *pool;

    if(size > 0x7FFFFFFF){
        LOG_E("parameter error");
        return NULL;
    }
    
    //分配FIFO的内存池
    pool = pvPortMalloc(size);
    if(!pool) {
        LOG_E("malloc failed");
        return NULL;
    }
    
    //分配FIFO句柄
    fifo = (fifo_t*)pvPortMalloc(sizeof(fifo_t));
    if(!fifo) {
        vPortFree(pool);
        LOG_E("malloc failed");
        return NULL;
    }
    
    //创建挂起线程的信号量
    fifo->suspend = xSemaphoreCreateBinary();
    if(!fifo->suspend) {
        vPortFree(pool);
        vPortFree(fifo);
        LOG_E("create semaphore failed");
        return NULL;
    }
    
    //初始化FIFO池
    fifo->pool = pool;
    fifo->size = size;
    //设置FIFO为空
    fifo->is_empty = 1;
    fifo->r_index = 0;
    fifo->w_index = 0;
    
    return fifo;
}

/**
* 复位FIFO
 *
 * @param fifo FIFO对象
 *
 * @return 成功返回ERR_NORMAL
 */
int32_t fifo_clear(fifo_t *fifo)
{
    taskENTER_CRITICAL();
    //设置FIFO为空
    fifo->is_empty = 1;
    fifo->r_index = 0;
    fifo->w_index = 0;
    taskEXIT_CRITICAL();
    
    return ERR_NORMAL;
}

/**
* 复位FIFO
 *
 * @param fifo FIFO对象
 *
 * @return 成功返回ERR_NORMAL
 */
int32_t fifo_clear_isr(fifo_t *fifo)
{
    UBaseType_t interrupt_status;
    
    portASSERT_IF_INTERRUPT_PRIORITY_INVALID();
    interrupt_status = portSET_INTERRUPT_MASK_FROM_ISR();
    //设置FIFO为空
    fifo->is_empty = 1;
    fifo->r_index = 0;
    fifo->w_index = 0;
    portCLEAR_INTERRUPT_MASK_FROM_ISR(interrupt_status);
    
    return ERR_NORMAL;
}

/**
 * 获取FIFO中的数据长度
 *
 * @param fifo FIFO对象
 *
 * @return 返回FIFO中的数据长度
 */
ssize_t fifo_length(fifo_t *fifo)
{
    ssize_t size;
    
    if(!fifo) {
        LOG_E("parameter error");
        return ERR_INVAL;
    }
    
    //获取FIFO长度
    taskENTER_CRITICAL();
    size = (ssize_t)__fifo_lenght(fifo);
    taskEXIT_CRITICAL();
    
    return size;
}

/**
 * 获取FIFO中的数据长度
 *
 * @param fifo FIFO对象
 *
 * @return 返回FIFO中的数据长度
 */
ssize_t fifo_length_isr(fifo_t *fifo)
{
    ssize_t size;
    UBaseType_t interrupt_status;
    
    if(!fifo) {
        LOG_E("parameter error");
        return ERR_INVAL;
    }
    
    //获取FIFO长度
    portASSERT_IF_INTERRUPT_PRIORITY_INVALID();
    interrupt_status = portSET_INTERRUPT_MASK_FROM_ISR();
    size = (ssize_t)__fifo_lenght(fifo);
    portCLEAR_INTERRUPT_MASK_FROM_ISR(interrupt_status);
    
    return size;
}

/**
 * 将数据强制写入FIFO
 *
 * @param fifo FIFO句柄
 * @param data 待写入的数据
 * @param size 数据长度
 *
 * @return 返回成功写入的字节数
 */
ssize_t fifo_in_force(fifo_t *fifo, const void *data, size_t size)
{
    ssize_t result;
    
    if((!fifo) || (!data) || (size == 0)) {
        LOG_E("parameter error");
        return ERR_INVAL;
    }
    
    //将数据写入FIFO
    taskENTER_CRITICAL();
    result = __fifo_in_force(fifo, data, size);
    taskEXIT_CRITICAL();
    
    //写入成功则唤醒第一个线程
    if(result > 0) {
        xSemaphoreGive(fifo->suspend);
    }
    
    return result;
}

/**
 * 将数据强制写入FIFO
 *
 * @param fifo FIFO句柄
 * @param data 待写入的数据
 * @param size 数据长度
 * @param task_woken 是否有高优先级任务进入就绪状态
 *
 * @return 返回成功写入的字节数
 */
ssize_t fifo_in_force_isr(fifo_t *fifo, const void *data, size_t size, BaseType_t *task_woken)
{
    ssize_t result;
    UBaseType_t interrupt_status;
    
    if(task_woken) {
        *task_woken = pdFALSE;
    }
    
    if((!data) || (!fifo) || (size == 0)) {
        LOG_E("parameter error");
        return ERR_INVAL;
    }
    
    //将数据写入FIFO
    portASSERT_IF_INTERRUPT_PRIORITY_INVALID();
    interrupt_status = portSET_INTERRUPT_MASK_FROM_ISR();
    result = __fifo_in_force(fifo, data, size);
    portCLEAR_INTERRUPT_MASK_FROM_ISR(interrupt_status);
    
    //写入成功则唤醒第一个线程
    if(result > 0) {
        xSemaphoreGiveFromISR(fifo->suspend, task_woken);
    }
    
    return result;
}

/**
 * 将数据写入FIFO
 *
 * @param fifo FIFO句柄
 * @param data 待写入的数据
 * @param size 数据长度
 *
 * @return 返回成功写入的字节数
 */
ssize_t fifo_in(fifo_t *fifo, const void *data, size_t size)
{
    size_t empty_lenght;
    ssize_t result;
    
    if((!fifo) || (!data) || (size == 0)) {
        LOG_E("parameter error");
        return ERR_INVAL;
    }
    
    taskENTER_CRITICAL();
    
    //获取FIFO空闲长度
    empty_lenght = fifo->size - __fifo_lenght(fifo);
    if(empty_lenght == 0)  {
        taskEXIT_CRITICAL();
        LOG_W("FIFO full");
        return  ERR_FULL;
    }
    
    if(size > empty_lenght) {
        size = empty_lenght;
    }
    //将数据写入FIFO
    result = __fifo_in_force(fifo, data, size);
    
    taskEXIT_CRITICAL();
    
    //写入成功则唤醒第一个线程
    if(result > 0) {
        xSemaphoreGive(fifo->suspend);
    }
    
    return result;
}

/**
 * 将数据写入FIFO
 *
 * @param fifo FIFO句柄
 * @param data 待写入的数据
 * @param size 数据长度
 * @param task_woken 是否有高优先级任务进入就绪状态
 *
 * @return 返回成功写入的字节数
 */
ssize_t fifo_in_isr(fifo_t *fifo, const void *data, size_t size, BaseType_t *task_woken)
{
    UBaseType_t interrupt_status;
    size_t empty_lenght;
    ssize_t result;
    
    if(task_woken) {
        *task_woken = pdFALSE;
    }
    
    if((!fifo) || (!data) || (size == 0)) {
        LOG_E("parameter error");
        return ERR_INVAL;
    }
    
    portASSERT_IF_INTERRUPT_PRIORITY_INVALID();
    
    interrupt_status = portSET_INTERRUPT_MASK_FROM_ISR();
    
    //获取FIFO空闲长度
    empty_lenght = fifo->size - __fifo_lenght(fifo);
    if(empty_lenght == 0)  {
        portCLEAR_INTERRUPT_MASK_FROM_ISR(interrupt_status);
        LOG_W("FIFO full");
        return  ERR_FULL;
    }
    
    if(size > empty_lenght) {
        size = empty_lenght;
    }
    //将数据写入FIFO
    result = __fifo_in_force(fifo, data, size);
    
    portCLEAR_INTERRUPT_MASK_FROM_ISR(interrupt_status);
    
    //写入成功则唤醒第一个线程
    if(result > 0) {
        xSemaphoreGiveFromISR(fifo->suspend, task_woken);
    }
    return result;
}

/**
 * 从FIFO读取数据
 *
 * @param fifo FIFO句柄
 * @param buf 存放数据的缓存
 * @param size 期望读取的字节数
 * @param timeout 最长等待时间
 *
 * @return 返回成功读取的字节数，失败返回错误码
 */
ssize_t fifo_out(fifo_t *fifo, void *buffer, size_t size, TickType_t timeout)
{
    portBASE_TYPE xresult;
    ssize_t result;
    size_t fifo_lenght;
    
    if((!fifo) || (!buffer) || (size == 0)) {
        LOG_E("parameter error");
        return ERR_INVAL;
    }
    
    //进入临界区
    taskENTER_CRITICAL();
    
    //获取FIFO数据长度
    fifo_lenght = __fifo_lenght(fifo);
    if(fifo_lenght == 0)
    {
        if(timeout == 0) {
            taskEXIT_CRITICAL();
            LOG_W("FIFO empty");
            return ERR_EMPTY;
        }
        //清空信号量，确保后面能挂起线程
        xSemaphoreTake(fifo->suspend, 0);
        taskEXIT_CRITICAL();

        //挂起当前线程，并进行调度
        xresult = xSemaphoreTake(fifo->suspend, timeout);
        if(xresult != pdTRUE) {
            LOG_W("wait timeout");
            return ERR_OVERTIME;
        }
        
        taskENTER_CRITICAL();
        //再次获取FIFO数据长度，因为线程可能会被信号唤醒
        fifo_lenght = __fifo_lenght(fifo);
        if(fifo_lenght == 0) {
            taskEXIT_CRITICAL();
            LOG_E("FIFO error");
            return ERR_EMPTY;
        }
    }
    
    if(size > fifo_lenght) {
        size = fifo_lenght;
    }
    //从FIFO中读取数据
    result = __fifo_out(fifo, buffer, size);
    
    taskEXIT_CRITICAL();
    
    return result;
}

/**
 * 从FIFO读取数据
 *
 * @param fifo FIFO句柄
 * @param buf 存放数据的缓存
 * @param size 期望读取的字节数
 *
 * @return 返回成功读取的字节数，失败返回错误码
 */
ssize_t fifo_out_isr(fifo_t *fifo, void *buffer, size_t size)
{
    UBaseType_t interrupt_status;
    size_t fifo_lenght;
    ssize_t result;
    
    if((!fifo) || (!buffer) || (size == 0)) {
        LOG_E("parameter error");
        return ERR_INVAL;
    }
    
    //进入临界区
    portASSERT_IF_INTERRUPT_PRIORITY_INVALID();
    interrupt_status = portSET_INTERRUPT_MASK_FROM_ISR();
    
    //获取FIFO数据长度
    fifo_lenght = __fifo_lenght(fifo);
    if(fifo_lenght == 0)
    {
        //退出临界区
        portCLEAR_INTERRUPT_MASK_FROM_ISR(interrupt_status);
        LOG_W("FIFO empty");
        return ERR_EMPTY;
    }
    
    if(size > fifo_lenght) {
        size = fifo_lenght;
    }
    //从FIFO中读取数据
    result = __fifo_out(fifo, buffer, size);
    
    //退出临界区
    portCLEAR_INTERRUPT_MASK_FROM_ISR(interrupt_status);
    
    return result;
}

/**
 * 将数据强制写入FIFO
 *
 * @param fifo FIFO句柄
 * @param data 待写入的数据
 * @param size 数据长度
 *
 * @return 返回成功写入的字节数
 */
ssize_t fifo_in_force_no_schedule(fifo_t *fifo, const void *data, size_t size)
{
    if((!fifo) || (!data) || (size == 0)) {
        LOG_E("parameter error");
        return ERR_INVAL;
    }
    
    //将数据写入FIFO
    taskENTER_CRITICAL();
    size = __fifo_in_force(fifo, data, size);
    taskEXIT_CRITICAL();
    
    return (ssize_t)size;
}

/**
 * 将数据强制写入FIFO
 *
 * @param fifo FIFO句柄
 * @param data 待写入的数据
 * @param size 数据长度
 *
 * @return 返回成功写入的字节数
 */
ssize_t fifo_in_force_no_schedule_isr(fifo_t *fifo, const void *data, size_t size)
{
    UBaseType_t interrupt_status;
    
    if((!data) || (!fifo) || (size == 0)) {
        LOG_E("parameter error");
        return ERR_INVAL;
    }
    
    //将数据写入FIFO
    portASSERT_IF_INTERRUPT_PRIORITY_INVALID();
    interrupt_status = portSET_INTERRUPT_MASK_FROM_ISR();
    size = __fifo_in_force(fifo, data, size);
    portCLEAR_INTERRUPT_MASK_FROM_ISR(interrupt_status);
    
    return (ssize_t)size;
}

/**
 * 将数据写入FIFO
 *
 * @param fifo FIFO句柄
 * @param data 待写入的数据
 * @param size 数据长度
 *
 * @return 返回成功写入的字节数
 */
ssize_t fifo_in_no_schedule(fifo_t *fifo, const void *data, size_t size)
{
    size_t empty_lenght;
    ssize_t result;
    
    if((!data) || (!fifo) || (size == 0)) {
        LOG_E("parameter error");
        return ERR_INVAL;
    }
    
    taskENTER_CRITICAL();
    
    //获取FIFO空闲长度
    empty_lenght = fifo->size - __fifo_lenght(fifo);
    if(empty_lenght == 0)  {
        taskEXIT_CRITICAL();
        LOG_W("FIFO empty");
        return ERR_FULL;
    }
    
    if(size > empty_lenght) {
        size = empty_lenght;
    }
    //将数据写入FIFO
    result = __fifo_in_force(fifo, data, size);
    
    taskEXIT_CRITICAL();
    
    return result;
}

/**
 * 从FIFO读取数据
 *
 * @param fifo FIFO句柄
 * @param buf 存放数据的缓存
 * @param size 期望读取的字节数
 *
 * @return 返回成功读取的字节数，失败返回错误码
 */
ssize_t fifo_in_no_schedule_isr(fifo_t *fifo, const void *data, size_t size)
{
    UBaseType_t interrupt_status;
    size_t empty_lenght;
    ssize_t result;
    
    if((!fifo) || (!data) || (size == 0)) {
        LOG_E("parameter error");
        return ERR_INVAL;
    }
    
    //进入临界区
    portASSERT_IF_INTERRUPT_PRIORITY_INVALID();
    interrupt_status = portSET_INTERRUPT_MASK_FROM_ISR();
    
    //获取FIFO空闲长度
    empty_lenght = fifo->size - __fifo_lenght(fifo);
    if(empty_lenght == 0)  {
        portCLEAR_INTERRUPT_MASK_FROM_ISR(interrupt_status);
        LOG_W("FIFO empty");
        return ERR_FULL;
    }
    
    if(size > empty_lenght) {
        size = empty_lenght;
    }
    //将数据写入FIFO */
    result = __fifo_in_force(fifo, data, size);
    
    portCLEAR_INTERRUPT_MASK_FROM_ISR(interrupt_status);
    
    return result;
}

/**
 * 从FIFO读取数据
 *
 * @param fifo FIFO句柄
 * @param buf 存放数据的缓存
 * @param size 期望读取的字节数
 *
 * @return 返回成功读取的字节数，失败返回错误码
 */
ssize_t fifo_out_no_schedule(fifo_t *fifo, void *buffer, size_t size)
{
    return fifo_out(fifo, buffer, size, 0);
}

/**
 * 从FIFO读取数据
 *
 * @param fifo FIFO句柄
 * @param buf 存放数据的缓存
 * @param size 期望读取的字节数
 *
 * @return 返回成功读取的字节数，失败返回错误码
 */
ssize_t fifo_out_no_schedule_isr(fifo_t *fifo, void *buffer, size_t size)
{
    return fifo_out_isr(fifo, buffer, size);
}
